#include "CallBack.h"
#include "rtthread.h"
#include "main.h"
#include "FFBWheelMain.h"
#include "Zy_Tools.h"
/*************************
The nodes states

  Initialisation  = 0x00, 
  Disconnected    = 0x01,
  Connecting      = 0x02,
  Preparing       = 0x02,
  Stopped         = 0x04,
  Operational     = 0x05,
  Pre_operational = 0x7F,
  Unknown_state   = 0x0F
*************************/
extern struct rt_mailbox mbSlaveId;
extern ffb_wheel_t ffbWheel;
/***********************************************************************
**callbacks frome canfestival  start
**这些回调看需求实现
***********************************************************************/
//states.c
void _initialisation(CO_Data* d)
{

}

void _preOperational(CO_Data* d)
{

    if (!(*(d->iam_a_slave)))
    {
        masterSendNMTstateChange (d, 0, NMT_Reset_Node);
    }
}

void _operational(CO_Data* d)
{

}

void _stopped(CO_Data* d)
{

}

//emcy.c
void _post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg, const UNS8 errSpec[5])
{
	(void)d;
	(void)nodeID;
	(void)errCode;
	(void)errReg;
	(void)errSpec;
}

//lifegrd.c
void _heartbeatError(CO_Data* d, UNS8 heartbeatID)
{
	(void)d;
	(void)heartbeatID;
}

uint8_t bootSlaveID;
//设备上线会通知此处
void _post_SlaveBootup(CO_Data* d, UNS8 SlaveID)
{
	bootSlaveID = SlaveID;
	rt_mb_send(&mbSlaveId,(rt_ubase_t)&bootSlaveID);
	(void)d;
}

void _post_SlaveStateChange(CO_Data* d, UNS8 nodeId, e_nodeState newNodeState)
{
	(void)d;
	(void)nodeId;
	(void)newNodeState;
}

void _nodeguardError(CO_Data* d, UNS8 id)
{
	(void)d;
	(void)id;
}

//sync.c
void _post_sync(CO_Data* d)
{
	(void)d;
}
void _post_TPDO(CO_Data* d)
{
	(void)d;
}

void canChangeBaudRate(CAN_PORT canhandle, char* baudrate)
{
	return;
}

//objacces.c
//需要保存的参数，回调函数
UNS32 _storeODSubIndex (CO_Data* d, UNS16 wIndex, UNS8 bSubindex)
{
	
  (void)d;
  (void)wIndex;
  (void)bSubindex;
  return OD_SUCCESSFUL;
}



/***********************************************************************
**callbacks from canfestival  end
***********************************************************************/


/***********************************************************************
**callbacks user defined start
***********************************************************************/
interval_t cbInterval;
UNS32 _cb_5064(CO_Data* d, const indextable* table, UNS8 bSubindex)
{
//	Calc_Interval(&cbInterval);
	metric_t* metric = &ffbWheel.axisX.metric.current;
	Axis* axis = &ffbWheel.axisX;
	metric->posRaw = *(int32_t*)table->pSubindex[1].pObject;	        // 此单位是1圈0x20000
	AxisUpdateMetrics(axis,metric->posRaw);
	return 0;
}

#ifdef WATCH_VAR
int16_t speedDegress;
#endif
UNS32 _cb_506C(CO_Data* d, const indextable* table, UNS8 bSubindex)
{
	metric_t* metric = &ffbWheel.axisX.metric.current;
	//这里直接给出速度，不进行滤波，换算成了游戏中的速度
//	metric->speed = (float)actualVel[0] /0x20000 * (-360.0);     // 单位0.1r/min
//	metric->speed = (float)actualVel[0] * (-360.0)/10/60 ;       // 反馈单位0.1r/min，换算成deg/s
	metric->speed = -(float)*(int32_t*)table->pSubindex[1].pObject*0.6 ;             // 反馈单位0.1r/min，换算成deg/s
#ifdef WATCH_VAR
	speedDegress = metric->speed;
#endif
	return 0;
}
/***********************************************************************
**callbacks user defined end
***********************************************************************/


/***********************************************************************
**ע��ص�����
***********************************************************************/							

void CANOpen_RegisterCB(void)
{
    // FFB的回调处理
	RegisterSetODentryCallBack(&Servo_OD_Data,0x5064,0x01,_cb_5064);
    RegisterSetODentryCallBack(&Servo_OD_Data,0x506C,0x01,_cb_506C);
}








